package nautilus.tank.coord;

import java.awt.Point;

public class Coordinator3D {
	private int scaleX, scaleY, scaleZ;
	
	//view port, in most cases, these values are 0, 0, with of view component,
	// height of component
	private int X0 = 0;
	private int Y0 = 0;
	private int viewWidth, viewHeight;
	
	// 0-position
	private int screenX,screenY;
	private Point O = new Point();
    private Point PX = new Point();
	private Point PY = new Point();
	private Point PZ = new Point();
    
    /** view matrix */
    private float[] viewMatrix = new float[16];
	private float[] rotatedViewMatrix = new float[16];
    
	/** Perspective matrix */
    private float[] matrixPerspective = new float[16]; 
	
	/** Matrix to translate object from local to world space */
    float[] model = new float[16];
	
	/** modelView = Mview * Mmodel */
    float[] modelView = new float[16];
	
	/** MVP matrix */
    float[] MVP = new float[16];
	
    /** rotation vector */
    private float rotateX, rotateY, rotateZ;
	/** angle of rotation in radian */
	private float theta = 0.0f;
	
	//Eyes vector & Camera coordinator system
	private float[] eye = {0.5f, 0.5f, 2.5f};
	private float[] lookAt = {0f, 0f, 0f};;
	private float[] upVector = new float[3];
	public float far = 10f; //<<=
	public float near = 1f; //<<=
    
    private float dvx = 2.5f, dvy = 2.5f, dvz = 2.5f;

    /**
	
		@param 
	*/    
    public Coordinator3D(int sx, int sy, int w, int h, 
					int scx, int scy, int scz) {
    	scaleX = scx;
        scaleY = scy;
        scaleZ = scz;
        screenX = sx;
        screenY = sy;
		
		X0 = 0;
		Y0 = 0;
		viewWidth = w;
		viewHeight = h;
        
        upVector[0] = 0f;
		upVector[1] = 1f;
		upVector[2] = 0f;
		
		float ratio = w*1.0f/h;
		System.out.println("[DEBUG] ratio = " + ratio);
		//l,t,r,b,n,f
    	//setFrustum(-ratio, ratio, ratio, -ratio, near, far);
		setFrustum(-5f, 5f, 5, -5, near, far);
		//setFrustum(-2f, 2f, 2, -2, near, far);
		
        Matrix3D.lookAtM(viewMatrix, 
        		eye[0], eye[1], eye[2], //eye (x,y, z)
        		lookAt[0], lookAt[1], lookAt[2],	//target 
        		upVector[0], upVector[1], upVector[2]		//up vector
        		);
				
		//reset rotation
		System.arraycopy(viewMatrix, 0, rotatedViewMatrix, 0, 16);
        
        calculateAxises();
    }
    
    public void onViewChanged(int x0, int y0, int w, int h){
    	this.screenX = x0;
    	this.screenY = y0;
    	this.viewWidth = w;
    	this.viewHeight = h;
    	
    	float ratio = w*1.0f/h;
    	setFrustum(-ratio, ratio, ratio, -ratio, near, far);
		Matrix3D.lookAtM(viewMatrix, 
        		eye[0], eye[1], eye[2], //eye (x,y, z)
        		lookAt[0], lookAt[1], lookAt[2],	//target, where we are looking at 
        		upVector[0], upVector[1], upVector[2]/* up vector*/	);
		calculateAxises();
    }
        
    public int getScaleX(){
    	return scaleX;
    }
     
    public void setScaleX(int v){
    	scaleX = v;
    }
        
    public int getScaleY() {
    	return scaleY;
    }
       
    public void setScaleY(int v) {
    	scaleY = v;
    }
        
    public int getScaleZ(){
    	return scaleZ;
    }
        
    public void setScaleZ(int v){
    	scaleZ = v;
    }
        
    public int getScreenX(){
    	return screenX;
    }
        
    public void setScreenX(int v){
    	screenX = v;
    }
        
    public int getScreenY(){
    	return screenY;
    }
        
    public void setScreenY(int v){
    	screenY = v;
    }
    
    public float[] getEye(){
		return eye;
	}
	
	public float[] getLookAt(){
		return lookAt;
	}
	
	public float getNear(){
		return near;
	}
	
	public float getFar(){
		return far;
	}
    
    public void setupCamera(float ex, float ey, float ez,
			float targetX, float targetY, float targetZ,
    		float ux, float uy, float uz){
			
    	eye[0] = ex;
    	eye[1] = ey;
    	eye[2] = ez;
    	
    	upVector[0] = ux;
		upVector[1] = uy;
		upVector[2] = uz;
		
		lookAt[0] = targetX;
		lookAt[1] = targetY;
		lookAt[2] = targetZ;
		
		//float ratio = w*1.0f/h;
		//System.out.println("[DEBUG] ratio = " + ratio);
		//setFrustum(l,t,r,b,n,f)
    	//setFrustum(-ratio, ratio, ratio, -ratio, near, far);
    	
		//Now, we got the view matrix
    	Matrix3D.lookAtM(viewMatrix, 
        		eye[0], eye[1], eye[2], //eye (x,y, z)
        		lookAt[0], lookAt[1], lookAt[2],	//target, where we are looking at 
        		upVector[0], upVector[1], upVector[2]/* up vector*/	);
				
		//reset rotation
		System.arraycopy(viewMatrix, 0, rotatedViewMatrix, 0, 16);
		
		calculateAxises();
    }
	
	private void setFrustum(float l, float t, float r, float b, float n, float f){
		near = n;
		far = f;
		Matrix3D.setIdentity(matrixPerspective);
		matrixPerspective[0]  = 2 * n / (r - l);
		matrixPerspective[2]  = (r + l) / (r - l);
		matrixPerspective[5]  = 2 * n / (t - b);
		matrixPerspective[6]  = (t + b) / (t - b);
		matrixPerspective[10] = -(f + n) / (f - n);
		matrixPerspective[11] = -(2 * f * n) / (f - n);
		matrixPerspective[14] = -1;
		matrixPerspective[15] = 0;
	}
	
	public float[] getUpVector(){
		return upVector;
	}
	
	public void setViewSize(int w, int h){
    	viewWidth = w;
    	viewHeight = h;
    }
        
    public void rotate(float a, float x, float y, float z){
    	rotateX = x;
        rotateY = y;
        rotateZ = z;
		
		Matrix3D.rotateM(model, a, rotateX, rotateY, rotateZ);
		Matrix3D.multiplyMM(rotatedViewMatrix, viewMatrix, model);
		
        /* after rotating, we have to re-calculate the axises on screen */
        calculateAxises();
    }
        
    public float[] getViewMatrix() {
    	return viewMatrix;
    }
        
    public Coordinator3D(){
    	scaleX = 100;
        scaleY = 100;
        screenX = 200;
        screenY = 70;
    }
        
    public void world2Screen(Point outP, float x, float y, float z) {
    	
    	float[] r = {0, 0, 0, 1};
      
        Matrix3D.setIdentity(model);
        Matrix3D.translateM(model, x, y, z);
		
		//DEBUG
		/*
		for(int i=0;i<4;i++){
			System.out.println("");
			for(int j=0;j<4;j++){
				System.out.print(model[i*4 + j] + " ");
			}
		}
		System.out.println("\n--------------------");
		*/
		
        Matrix3D.multiplyMM(modelView, rotatedViewMatrix, model);
        Matrix3D.multiplyMM(MVP, matrixPerspective, modelView);
		
		/** Here we got clip view point */
        Matrix3D.multiplyMV(r, MVP, new float[]{x, y, z, 1});
			
		/**
			calculate p(ndc)
			I'm not sure, please check
		*/
		if(r[3] != 0){
			r[0] = r[0]/r[3];
			r[1] = r[1]/r[3];
			r[2] = r[2]/r[3];
		}
		
		//DEBUG
		//System.out.println("(" + r[0] + ", " + r[1] + ", " + r[2] + ", w=" + r[3] +")");
		
		//float hw = viewWidth/2.0f;
		//float hh = viewHeight/2.0f;
		
		//I'm not sure, please check
		/** 
			Apply scaling and translating to fix the screen coordinate
		*/
		float[] w = { (scaleX*r[0]) + screenX, screenY - scaleY*r[1], (far-near)*r[2]/2.0f + (far + near)/2.0f};
		
		//DEBUG
		//System.out.println("(" + w[0] + ", " + w[1] + ", " + w[2] + ")");
		
    	//float xs = r[0] * scaleX + screenX;
    	//float ys = screenY - (r[1] * scaleY);
        //return (new Point( (int)Math.round(xs), (int)Math.round(ys)));
		
		//outP.setLocation(w[0], w[1]);
		outP.setLocation((0.5f * r[0] + 0.5f)*viewWidth, viewHeight - (0.5f * r[1] + 0.5f)*viewHeight /*(0.5f * r[1] + 0.5f)*viewHeight*/);
		//return (new Point( (int)Math.round(w[0]), (int)Math.round(w[1])));
    }
	
	public void world2Screen(float[] outP, int offset, float x, float y, float z, float theta) {
    	
    	float[] r = {0, 0, 0, 1};
        Matrix3D.setIdentity(model);
        //Matrix3D.translateM(model, x, y, z);
        //Matrix3D.rotateM(model, theta, 0, 0.2f, 0);
		
        Matrix3D.multiplyMMM(MVP, matrixPerspective, rotatedViewMatrix, model);
        //Matrix3D.multiplyMM(modelView, rotatedViewMatrix, model);
        //Matrix3D.multiplyMM(MVP, matrixPerspective, modelView);
		
		/** Here we got clip view point */
        Matrix3D.multiplyMV(r, MVP, new float[]{x, y, z, 1});
			
		/**
			calculate p(ndc)
			I'm not sure, please check
		*/
		if(r[3] != 0){
			r[0] = r[0]/r[3];
			r[1] = r[1]/r[3];
			r[2] = r[2]/r[3];
		}

		/** 
			Apply scaling and translating to fix the screen coordinate
		*/
		//float[] w = { (scaleX*r[0]) + screenX, screenY - scaleY*r[1], (far-near)*r[2]/2.0f + (far + near)/2.0f};
		//outP[offset] 	= scaleX * r[0] + screenX;
		//outP[offset+1] 	= screenY - scaleY * r[1];
		
		outP[offset] 		= (0.5f * r[0] + 0.5f)*viewWidth;
		outP[offset+1] 	= viewHeight - (0.5f * r[1] + 0.5f)*viewHeight;
    }
        
        
	/**
		At this point, I follow the flow of OpenGL pipeline
		(x, y, z)eye = Mmodelview . Object = Mmodel.Mview.(x,y,z)object
		
		MVP = P . V . M 
		p(clip view) = MVP . P
		p(ndc) = p(clip view)/width(clip)
		
		@param outP output point on screen
	*/
    public void world2Screen(Point outP, float[] p) {
        float[] r = {0, 0, 0, 1};
       
        Matrix3D.setIdentity(model);
        Matrix3D.translateM(model, p[0], p[1], p[2]);
        
        Matrix3D.multiplyMMM(MVP, matrixPerspective, rotatedViewMatrix, model);
		
		/** Here we got clip view point */
        Matrix3D.multiplyMV(r, MVP, p);
		
		/** calculate p(ndc) */
		/**
			calculate p(ndc)
			I'm not sure, please check
		*/
		if(r[3] != 0){
			r[0] = r[0]/r[3];
			r[1] = r[1]/r[3];
			r[2] = r[2]/r[3];
		}
		
		//float hw = viewWidth/2.0f;
		//float hh = viewHeight/2.0f;
		
		//I'm not sure, please check
		/** 
			Apply scaling and translating to fix the screen coordinate
		*/
		//float[] w = { (scaleX*r[0]) + screenX, screenY - scaleY*r[1], (far-near)*r[2]/2.0f + (far + near)/2.0f};
		
        //float xs = r[0] * scaleX + screenX;
        //float ys = screenY - (r[1] * scaleY);
        //return (new Point( (int)Math.round(xs), (int)Math.round(ys)));
		
		outP.setLocation((0.5f * r[0] + 0.5f)*viewWidth, viewHeight - (0.5f * r[1] + 0.5f)*viewHeight /*(0.5f * r[1] + 0.5f)*viewHeight*/);
		//return (new Point( (int)Math.round(w[0]), (int)Math.round(w[1])));
    }
        
    public float[] screen2World(int x, int y, float z){
    	float[] result = new float[3];
        result[0] = (screenX + x)/scaleX;
        result[1] = (screenY - y)/scaleY;
        result[2] = z;
        return result;
    }
        
    public float[] screen2World(Point p, float z) {
    	float[] result = new float[3];
        result[0] = (screenX + p.x)/scaleX;
        result[1] = (screenY - p.y)/scaleY;
        result[2] = z;
        return result;
    }
        
    /* Calculate the SCREEN axises 
    */
    private void calculateAxises() {
    	//world vector for axises
        //float[] X = {dvx, 0.0f, 0.0f, 1};
        //float[] Y = {0.0f, dvy, 0.0f, 1};
        //float[] Z = {0.0f, 0.0f, dvz, 1};
		float[] X = {1 ,0.0f, 0.0f, 1};
        float[] Y = {0.0f, 1, 0.0f, 1};
        float[] Z = {0.0f, 0.0f, 1, 1};
        float[] buffer ={0.0f, 0.0f, 0.0f, 1};
		
        world2Screen(PX, X);
        world2Screen(PY, Y);
        world2Screen(PZ, Z);
		world2Screen(O, buffer);
		screenX = (int)(O.getX());
		screenY = (int)(O.getY());
    }
	
	public Point getPX(){return PX;}
	public Point getPY(){return PY;}
	public Point getPZ(){return PZ;}
    
    public Point getConner(){
    	return O;
    }
	
	public void getViewAxis(float[] out, int axis) {
		if(axis == 1){ //X axis
			out[0] = viewMatrix[0];
			out[1] = viewMatrix[4];
			out[2] = viewMatrix[8];
			return;
		}
		
		if(axis == 2){ //Y axis
			out[0] = viewMatrix[1];
			out[1] = viewMatrix[5];
			out[2] = viewMatrix[9];
			return;
		}
		
		if(axis == 3){ //Z axis
			out[0] = viewMatrix[2];
			out[1] = viewMatrix[6];
			out[2] = viewMatrix[10];
			return;
		}
	}
	
	/**
		Calculate dot (scalar) product of two vector
		some formular: 
			v.w = |v|.|w|.cos(theta)
			v.w = v1*w1 + v2*w2 + v3*w3
	*/
	public static float dot(float x1, float y1, float z1, float x2, float y2, float z2){
		return x1*x2 + y1*y2 + z1*z2;
	}
	
	/**
		Calculate angle of two vectors using DOT product
		
		v1.v2 = |v1|.|v2|.cos(alpha) = v1.x*v2.x + v1.y*v2.y + v1.z*v2.z
		=> alpha = arcos( (v1.v2)/(|v1|.|v2|) );
		@param
	*/
	public static float getAngleInRadian(float[] v1, float[] v2){
		double dot = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
		return (float)Math.acos( dot/( Math.sqrt(v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2]) * 
							Math.sqrt(v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]) ) );
	}
	
	/**
		The orientation
		test of four points a, b, c, d checks the mutual position of the point d and a plane Q passing
		through the points a, b, c.
		
		If the result of the test orient(a, b, c, d) is positive, negative, respectively, the point d lies
		above, bellow, respectively, the plane Q. If the result is equal to zero, d lies in the plane Q.
		
		@return
			
	*/
	public static float orient(float[] a, float[] b, float[] c, float[] d){
		float[] m = {	a[0], a[1], a[2], 1,
							b[0], b[1], b[2], 1,
							c[0], c[1], c[2], 1,
							d[0], d[1], d[2], 1};
		return Matrix3D.det(m, 4);
	}
	
	public static float orient(float ax, float ay, float az, 
											float bx, float by, float bz, 
											float cx, float cy, float cz,
											float dx, float dy, float dz){
		float[] m = {	ax, ay, az, 1,
							bx, by, bz, 1,
							cx, cy, cz, 1,
							dx, dy, dz, 1};
		return Matrix3D.det(m, 4);
	}
	
	/**
		If the result of the test in sphere(a, b, c, d, e) is negative, positive, respectively, the point e
		lies outside, inside, respectively, the circumscribed sphere of the points a, b, c, d. If the result
		is equal to zero, e lies on the circumscribed sphere.
	*/
	public static float inSphere(float[] a, float[] b, float[] c,
									float[] d, float[] e){
		float[] A = {	a[0], a[1], a[2], a[0]*a[0] + a[1]*a[1] + a[2]*a[2], 1,
							b[0], b[1], b[2], a[0]*a[0] + b[1]*b[1] + b[2]*b[2], 1,
							c[0], c[1], c[2], a[0]*a[0] + c[1]*c[1] + c[2]*c[2], 1,
							d[0], d[1], d[2], a[0]*a[0] + d[1]*d[1] + d[2]*d[2], 1,
							e[0], e[1], e[2], e[0]*e[0] + e[1]*e[1] + e[2]*e[2], 1};
		
		float det = Matrix3D.det(A, 5) * orient(a, b, c, d);
		
		return det;
	}
	
	public static float inSphere(	float ax, float ay, float az, 
												float bx, float by, float bz, 
												float cx, float cy, float cz,
												float dx, float dy, float dz, 
												float ex, float ey, float ez){
		float[] A = {	ax, ay, az, ax*ax + ay*ay + az*az, 1,
							bx, by, bz, ax*ax + by*by + bz*bz, 1,
							cx, cy, cz, ax*ax + cy*cy + cz*cz, 1,
							dx, dy, dz, ax*ax + dy*dy + dz*dz, 1,
							ex, ey, ez, ex*ex + ey*ey + ez*ez, 1};
		
		float det = Matrix3D.det(A, 5) * orient(	ax,  ay,  az, 
																bx,  by,  bz, 
																cx,  cy,  cz,
																dx,  dy,  dz);
		
		return det;
	}
}